package clubhouse.base;

import java.io.FileNotFoundException;
import java.text.DecimalFormat;
import java.util.ListIterator;
import java.util.Vector;

import org.apache.log4j.PropertyConfigurator;


import clubhouse.antislam.AntiSLAM;
import clubhouse.communication.ReceptionFifo;
import clubhouse.communication.RobotCommunication;
import clubhouse.communication.TransmissionFifo;
import clubhouse.control.DataChecker;
import clubhouse.control.ErrorRetriever;
import clubhouse.control.LandmarkGenerator;
import clubhouse.error.TransmissionError;
import clubhouse.kalmanfilter.Ekf;
import clubhouse.motioncommand.MotionCommand;
import clubhouse.motioncommand.MotionPlan;
import clubhouse.visualisation.WSNexperimentation;

/**
 * @author Anis Darwich & Lionel Croix - Simulation and experimentation on Robot in WSN - WnSN Lab - SJTU
 */
public class Base implements Runnable {
	
	/********Simulation Parameters Constants********/
	/**Set it true to display debug logs in console.**/
	public static final boolean DEBUG = false;
	/**Robot's final destination point.**/
	public static final double[] FINALDESTINATION = new double[]{70,30};
	/**Distance value for which the final destination is considering reached.**/
	public static final double FINALDISTANCE = 2;
	/**Initial Position of the robot for scenarios 2 and 3.**/
	public static final double[] ROBOTINITIALPOSITION = new double[] {-20, 30};
	/**Initial Position of the robot for scenario 1.**/
	public static final double[] ROBOTINITIALPOSITION_SC1 = new double[] {-10, 20};
	/**Set it to true to enable Xbee communication.**/
	private boolean COMMUNICATION = false;
	/**********************************************/
	
	/*********Landmark Generation Constants********/
	/**Landmark Generation Constant - Generate network randomly.**/
	public static final int RANDOM = 0;
	/**Landmark Generation Constant - Generate network by reading it from the specified file in the "networks" folder.**/
	public static final int READFROMFILE = 1;
	/**Landmark Generation Constant - Generate network by getting user landmarks from the panel.**/
	public static final int READFROMPANEL = 2;
	/**Landmark Generation Constant - Save the current network in specified file in the "networks" folder.**/
	public static final int SAVECONFIG = 3;
	/**Landmark Generation Constant - Experimentation value, use real nodes received by radio.**/
	public static final int REALNETWORK = 3;
	/**********************************************/
	
	/*********Complete Simulation Constants********/
	/**Complete Simulation Constant - Not disturbed trajectory (or trajectory without Anti-SLAM during Anti-SLAM comparison).**/
	public static final int INITIALTRAJECTORY = 0;
	/**Complete Simulation Constant - Trajectory with error enabled (or trajectory with Translation Anti-SLAM strategy during Anti-SLAM comparison).**/
	public static final int ERRORTRAJECTORY = 1;
	/**Complete Simulation Constant - Trajectory with error and EKF enabled (or trajectory with Rotation Anti-SLAM strategy during Anti-SLAM comparison).**/
	public static final int EKFTRAJECTORY = 2;
	/**Complete Simulation Constant - Trajectory with error, EKF and Anti-SLAM enabled (or trajectory with Hybrid Anti-SLAM strategy during Anti-SLAM comparison).**/
	public static final int ANTISLAMTRAJECTORY = 3;
	/**********************************************/
	
	/** The number of consecutive simulations to run. **/
	private int numberOfSimulations;
	/**The error attenuation factor.*/
	private int errorFactor;
	/**The object executing all communication tasks.*/
	Runnable xbee;
	/**The Xbee communication independent thread.*/
	private Thread t;
	/**The simulation state, true if simulating, false otherwise.*/
	private boolean simulation = false;
	/**The current scenario.*/
	private int scenario;
	/**The enabling error state, true if error process is enabled, false otherwise.*/
	private boolean enableError;
	/**The enabling EKF state, true if EKF process is enabled, false otherwise.*/
	private boolean enableEkf;
	/**The enabling Anti-SLAM state, true if Anti-SLAM process is enabled, false otherwise.*/
	private boolean antiSlam;
	/**The enabling Complete simulation state, true if simulating the 4 stage complete simulation, 
	 * false otherwise.*/
	private boolean completeSimulation;
	/**The enabling Comparing Anti-SLAM state, true if simulating the comparing Anti-SLAM simulation, 
	 * false otherwise.*/
	private boolean compareAntiSlam;
	/**The current value of the Anti-SLAM strategy.**/
	private int antiSlamType = AntiSLAM.HYBRID;
	/**Final destination*/
	//private double[] finalDestination;
	/**The object choosing the next destination at each step.*/
	private MotionPlan motionControl;
	/**The object applying error on received coordinates.*/
	private TransmissionError mistaker;
	/**The object applying Anti-SLAM error on received coordinates*/
	private AntiSLAM counterSlam;
	/**The object executing EKF filter.*/
	private Ekf filter;
	
	/**The reception queue of Xbee device.**/
	public ReceptionFifo reception;
	/**The transmission queue of Xbee device.**/
	public TransmissionFifo transmission;
	
	/**The robot instance.**/
	public Robot rover;
	/**Object containing known landmarks (node id, x-coordinate and y-coordinate).**/
	public Landmarks nodes;
	/**Object containing the generated network, managed by 'manageLandmarks' function.**/
	public LandmarkGenerator landmarkGenerator = new LandmarkGenerator();
	
	/**The interface of management and visualization.**/
	public WSNexperimentation visual;
	/**Object containing generated error pool.**/
	public ErrorRetriever storeError;

	/**The set of real Landmarks, as generated or as received before processing them.*/
	private Vector<double[]> realLandmarks = null;
	/**The robot detection state, true if detected, false otherwise.*/
	private boolean isDetected = false;
	
	/**
	 * Constructs the Base station linked to the specified interface.
	 * @param v the interface used to control and visualize the simulation.
	 */
	public Base(WSNexperimentation v) {
		this.visual = v;
	}
	
	/**
	 * Control simulation (start/stop) as chosen by user in GUI.
	 * Simulation is launched if simul is true, and stopped otherwise.
	 * @param simul the value of simulation parameter to set.
	 */
	public void setSimulation(boolean simul) {
		this.simulation = simul;
	}
	
	/**
	 * Tests if the simulation is running.
	 * @return true if is simulating, false otherwise.
	 */
	public boolean isSimulating() {
		return(this.simulation);
	}
	
	/**
	 * Activate anti-slam when chosen in GUI.
	 * Anti-SLAM is activated if enableAntiSlam is true, and stopped otherwise.
	 * @param enableAntiSlam the value of Anti-SLAM parameter to set.
	 */
	public void setAntiSlam(boolean enableAntiSlam) {
		this.antiSlam = enableAntiSlam;
	}
	
	/**
	 * Activate error when chosen in GUI.
	 * @param enabledError the value of enabling error parameter to set.
	 */
	public void setError(boolean enabledError) {
		this.enableError = enabledError;
	}
	
	/**
	 * Set the number of simulations to run to the specified value in the corresponding field in GUI.
	 * @param number the number of simulations to run
	 */
	public void setNumberOfSimulations(int number) {
		this.numberOfSimulations = number;
	}
	
	/**
	 * Set error factor to the specified value in the corresponding field in GUI.
	 * @param error the error factor to set.
	 */
	public void setErrorFactor(int error) {
		this.errorFactor = error;
	}
	
	/**
	 * Set scenario type to the specified value in the corresponding field in GUI.
	 * @param scenario the value of current simulation scenario.
	 */
	public void setScenario(int scenario) {
		this.scenario = scenario;
	}
	
	/**
	 * Activate ekf when chosen in GUI.
	 * @param enableEkf the value of enabling EKF parameter to set.
	 */
	public void setEkf(boolean enableEkf) {
		this.enableEkf = enableEkf;
	}
	
	/**
	 * Set simulation mode (simple simulation/ 4 stage comparative simulation) as chosen by user in GUI.
	 * @param simulation the value of complete simulation parameter.
	 */
	public void setCompleteSimulation(boolean simulation) {
		this.completeSimulation = simulation;
	}
	
	/**
	 * Set Anti-SLAM mode (simple simulation/ 4 stage comparative simulation) as chosen by user in GUI.
	 * @param comparing the value of comparing Anti-SLAM parameter.
	 */
	public void setComparingAntiSlam(boolean comparing) {
		this.compareAntiSlam = comparing;
	}
	
	/**
	 * Set Anti-SLAM type.
	 * @param antiSlamType the value of the Anti-SLAM type.
	 */
	public void setAntiSlamType(int antiSlamType) {
		this.antiSlamType = antiSlamType;
	}
	
	/**
	 * Set the detection status of robot for the current simulation.
	 * @param detected true if the robot has been detected (is in a node's acting Range), false otherwise.
	 */
	public void setDetected(boolean detected) {
		this.isDetected = detected;
	}
	
	/**
	 * Enable or disable the communication module for the current simulation
	 * @param isReal true if we are using the communication module and the real landmarks
	 */
	public void setCommunication(boolean isReal) {
		this.COMMUNICATION = isReal;
	}
	
	/**
	 * Initialize error block. Create instance and set parameters.
	 */
	public void initError() {	
		this.mistaker = new TransmissionError();
		this.storeError = new ErrorRetriever();
		this.mistaker.setErrorFactor(this.errorFactor);
	}
	
	/**
	 * Initialize ekf block. Create instance and set parameters.
	 */
	public void initEkf() {
		this.filter = new Ekf();
		this.filter.initCalcul();
		this.filter.setInitialPosition(rover.getX(), rover.getY(), 0, 0);
	}
	
	/**
	 * Initialize anti-slam block. Create instance and set parameters.
	 */
	public void initAntiSlam(int AntiSlamType){
		this.counterSlam = new AntiSLAM(this.scenario,this.realLandmarks,AntiSlamType);
	}
	
	/**
	 * Initialize motion control block. Create instance and set parameters.
	 */
	public void initMotion() {
		this.motionControl = new MotionPlan(this.scenario);
	}
	
	/**
	 * Initialize main block. Create landmark instance and set robot's initial position according to the scenario.
	 */
	public void initMain() {
		/*initialize robot's position*/
		if (this.scenario == 1){
			this.rover = new Robot(Base.ROBOTINITIALPOSITION_SC1[0],Base.ROBOTINITIALPOSITION_SC1[1],0);
		} else {
			this.rover = new Robot(Base.ROBOTINITIALPOSITION[0], Base.ROBOTINITIALPOSITION[1], 0);
		}

		/*initialize known landmarks*/
		this.nodes = new Landmarks();

		/*initialize parameters*/
		this.simulation = true;
		this.isDetected = false;
	}

	/**
	 * Initialize communication block. Create instance and set parameters.
	 */
	public void initCommunication(){
		try {
			this.reception = new ReceptionFifo();
			this.transmission = new TransmissionFifo();
			Runnable xbee = new RobotCommunication(this.reception, this.transmission);
			this.t = new Thread (xbee);
			this.t.start();
		} catch (Exception e) {
			e.printStackTrace();
		}
	}
	
	/**
	 * Set/Load/Create/Save landmarks as chosen by user in GUI in the corresponding fields.
	 * 
	 * @param action the action to do (Generate randomly, Load file, Save in file or Generate from panel).
	 * @param fileName the name of the file to read or save.
	 * @param numberOfNodes the number of nodes to generate in case of random generation.
	 * @param userLandmarks the user landmarks in case of generation from the panel.
	 * @throws FileNotFoundException if the file to read is not existing in "networks" folder.
	 */
	public void manageLandmarks(int action, String fileName, int numberOfNodes, Vector<double[]> userLandmarks) throws FileNotFoundException{

		switch (action) {
		case Base.RANDOM:
			//random
			this.landmarkGenerator.setNumberOfNode(numberOfNodes);
			this.realLandmarks = this.landmarkGenerator.generateNetwork();
			break;
		case Base.READFROMFILE:
			//read from file
			this.realLandmarks = this.landmarkGenerator.readScenario(fileName);
			break;
		case Base.READFROMPANEL:
			//read from panel
			this.realLandmarks = userLandmarks;
			break;
		case Base.SAVECONFIG:
			//save current configuration
			this.landmarkGenerator.writeScenario(fileName, userLandmarks);
			break;
		default:
			break;
		}
		//this.finalDestination[0] = Base.maxXCoordinate(realLandmarks);
		//this.finalDestination[0] = Base.maxYCoordinate(realLandmarks);
		
		this.visual.setRealLandmarks(this.realLandmarks);
	}

	/**
	 * Stop simulation.
	 */
	public void stopRoutine() {
		setSimulation(false);
	}
	
	/**
	 * Stop simulation and reInitialize parameters.
	 */
	public void clearRoutine() {
		this.isDetected = false;
		stopRoutine();
	}
	
	/**
	 * Resume simulation when stopped and not reInitialized.
	 */
	public void resumeRoutine() {
		setSimulation(true);
		if (this.COMMUNICATION) this.t.notify();
	}
	
	/**
	 * Run the simulation (startRoutine).
	 */
	public void run() {
		startRoutine();
	}	
	
	/**
	 * Calls the correct simulation routine according to user parameters.
	 * 		If "Simulate" checkbox checked, run executeRoutine1
	 * 		Else if "Anti-SLAM comparison" checkbox checked, run executeRoutine0
	 * 		Run executeRoutine2 otherwise
	 */
	public void startRoutine() {
		PropertyConfigurator.configure("log4j.properties");
		setSimulation(true);
		
		for(int i=0; i<this.numberOfSimulations; i++) {
			this.visual.setSimulationNumber(i);
			if (this.completeSimulation) {
				this.executeRoutine1();
			} else if (this.compareAntiSlam){
				try {
					this.manageLandmarks(Base.RANDOM, null, this.realLandmarks.size(), null);
				} catch (FileNotFoundException e) {
					// Will never appear
					e.printStackTrace();
				}
				this.executeRoutine0();
			} else this.executeRoutine2();
		}
		this.visual.addStopInfo(false, true);
	}
	
	/**
	 * Executes the 4 stage comparative Anti-SLAM.
	 * The simulation is run 4 times each time with transmission error & ekf but with different Anti-SLAM type:
	 * 		1- without Anti-SLAM
	 * 		2- Translation Anti-SLAM
	 * 		3- Rotation Anti-SLAM
	 * 		4- Hybrid Anti-SLAM
	 */
	public void executeRoutine0(){
		
		/***Simulation's variables declaration***/
		double [] tempReceive = null;
		double [] tempLandmark = null;
		double [] receiveCoord = new double[2];
		double [] antiSlamedCoord = new double[2];
		double [] errCoord, finalCoord;
		ListIterator<double[]> li;
		double distance, rotation, trigo;
		int coordIndex = 0, step = 0;
		DecimalFormat df = new DecimalFormat("#####.##");
		/****************************************/
		
		/***Initialize components***/
		initMain();
		initMotion();	
		initError();
		if (this.COMMUNICATION) initCommunication();
		/***************************/
		
		try {
			for (int trajectory=0;trajectory<4;trajectory++){
				/******************************************************/
				/********************INITIALISATION********************/
				/******************************************************/
				//Initialize robot
				if (this.scenario == 1) this.rover.setPosition(Base.ROBOTINITIALPOSITION_SC1[0], Base.ROBOTINITIALPOSITION_SC1[1], 0);
				else this.rover.setPosition(Base.ROBOTINITIALPOSITION[0], Base.ROBOTINITIALPOSITION[1], 0);
				this.rover.storeCorrectPosition(trajectory);
				
				//reInitialize components
				this.nodes.reInitializeLandmarks();
				initEkf();
				initMotion();
				
				//Initialize error (recording or pool)
				step = 0;
				
				//Initialize common parameters
				this.isDetected = false;
				this.enableError = true;
				this.enableEkf = true;
				this.antiSlam = true;
				
				switch (trajectory){
					case Base.INITIALTRAJECTORY:
						if (Base.DEBUG) System.out.println("(executeRoutine1) Without Anti-SLAM");

						//Initialize error pool
						this.storeError.generateErrorPool();
						this.antiSlam = false;
						break;
					case Base.ERRORTRAJECTORY:
						if (Base.DEBUG) System.out.println("(executeRoutine1) With Translation Anti-SLAM");	
						
						//Initialize custom parameters
						this.antiSlamType = AntiSLAM.TRANSLATION;
						this.initAntiSlam(antiSlamType);
						
						break;
					case Base.EKFTRAJECTORY:
						if (Base.DEBUG) System.out.println("(executeRoutine1) With Rotation Anti-SLAM");
						
						//Initialize custom parameters
						this.antiSlamType = AntiSLAM.ROTATION;
						this.initAntiSlam(antiSlamType);
						break;
					case Base.ANTISLAMTRAJECTORY:
						if (Base.DEBUG) System.out.println("(executeRoutine1) With Hybrid Anti-SLAM");
						
						//Initialize custom parameters
						this.antiSlamType = AntiSLAM.HYBRID;
						this.initAntiSlam(antiSlamType);
						break;
					default:
						break;
				}
				
				/******************************************************/
				/************************RUNNING***********************/
				/******************************************************/
				while ((Base.euclideanDist(this.rover.getX(),this.rover.getY(),Base.FINALDESTINATION[0],Base.FINALDESTINATION[1])>Base.FINALDISTANCE)&&(!this.isDetected)&&(step<1000)) { //end
					if (!this.simulation){
						Thread.sleep(1000);
					} else {
						this.visual.addDebugLog(DataChecker.initialPosition, this.rover.getX(),this.rover.getY(),this.rover.getr());
						
						/***Learn landmark if in communication Range***/
						Vector<double[]> receivedLandmarks = null;
						if (this.COMMUNICATION) {	//In this case, realLandmarks = received landmarks
							receivedLandmarks = this.reception.getReceivedLandmarks();					
							//Displaying received landmarks
							ListIterator<double[]> receivedLandmarkList = receivedLandmarks.listIterator();
							while (receivedLandmarkList.hasNext()) {
								double[] landmark = receivedLandmarkList.next();
								this.visual.addMsgLog("[RECEIVED]\t[DATA]\t"+landmark[0]+"\t"+landmark[1]+"\t"+landmark[2]+"\n");
							}
						} else receivedLandmarks = this.realLandmarks; //Faking receiving landmark
						
						//Faking communication ranges: store close landmarks
						ListIterator<double[]> landmarkList = receivedLandmarks.listIterator();
						while (landmarkList.hasNext()) {
							double[] landmark = landmarkList.next();
							if ((Base.euclideanDist(this.rover.getX(),this.rover.getY(),landmark[1],landmark[2])<=DataChecker.communicationRange)&&(!this.nodes.knownLandmark((int)landmark[0], landmark[1], landmark[2]))) this.nodes.storeLandmark((int)landmark[0], landmark[1], landmark[2]);	
							
						}
						//Set Real Landmarks in case of communication
						if (this.COMMUNICATION) {
							this.realLandmarks = this.nodes.getRealLandmarks();
							this.visual.setRealLandmarks(this.realLandmarks);
							if (this.antiSlam) this.counterSlam.setRealLandmarks(this.realLandmarks);
						}
						/*********************************************/
						
						/*****Process data reception*****/
						for (int j=0;j<this.nodes.size();j++){
							tempReceive = this.nodes.getRealLandmarks().get(j);
							//Receiving nodes' coord
							coordIndex = (int)tempReceive[0];
							receiveCoord[0] = tempReceive[1];
							receiveCoord[1] = tempReceive[2];
						
							//Apply Anti-SLAM error
							if (this.antiSlam){ 
								antiSlamedCoord = this.counterSlam.applyAntiSlam(coordIndex,receiveCoord,this.rover.getX(), this.rover.getY());
								this.visual.addDebugLog(DataChecker.antiSlam, coordIndex,receiveCoord[0],receiveCoord[1],antiSlamedCoord[0],antiSlamedCoord[1],this.antiSlamType);					
							} else {
								antiSlamedCoord = receiveCoord;
							}
						
							//Apply error
							if (this.enableError){
								double err = (this.storeError.getErrorFromPool(step))[1];
								if (Base.DEBUG) System.out.println("(executeRoutine1) Used error "+err+ " during step "+step);
								errCoord = this.mistaker.applyErrorFromPool(err, this.rover.getX(), this.rover.getY(), antiSlamedCoord[0], antiSlamedCoord[1]);
								this.visual.addDebugLog(DataChecker.error, coordIndex,receiveCoord[0],receiveCoord[1],errCoord[0],errCoord[1]);
								step++;
							} else {
								errCoord = antiSlamedCoord;
							}
							
							//Store final Coord
							finalCoord = errCoord;
							this.nodes.replaceLandmark(coordIndex, finalCoord[0], finalCoord[1]);
							tempReceive = null;
						}
			        
						//Display all landmarks
						li = this.nodes.getLandmarks().listIterator();
						if (Base.DEBUG) System.out.println("(executeRoutine1) We have "+this.nodes.size()+" landmarks");
						while(li.hasNext()){
							tempLandmark = li.next();
							this.visual.addDebugLog(DataChecker.landmark,tempLandmark[0],this.nodes.size(),tempLandmark[1],tempLandmark[2]);
						}
				        /********************************/
					
						/***************Apply EKF***********/
						if ((this.enableEkf) && (this.nodes.size()>1)){	
							this.filter.setMotion(this.rover.getX(), this.rover.getY(), 0, 0);
					        this.filter.initMeasurement(this.nodes.getLandmarks());
					        this.filter.calcul();
					        this.rover.setPosition(this.filter.getMEstimate()[0][0], this.filter.getMEstimate()[1][0], 0);
					        this.rover.storeCorrectPosition(trajectory);	
							
					        this.visual.addDebugLog(DataChecker.ekf,this.rover.getX(),this.rover.getY(),0, 0);
					        if (Base.DEBUG) System.out.println("(executeRoutine1) Exit of EKF x: "+df.format(this.rover.getX())+ " Y: "+df.format(this.rover.getY())); 
						}
						/************************************/
						
				        /*****Calculate next motion step*****/
						this.motionControl.getNextPulses(this.rover.getX(),this.rover.getY(),this.rover.getr(),this.nodes);
						distance = this.motionControl.getPulse_dist();
						rotation = this.motionControl.getPulse_rot();
						trigo = this.motionControl.getTrigo();
						/************************************/
					
						/****************Logs***************/
						this.visual.addDebugLog(DataChecker.destination,this.motionControl.getDestinationCoord()[0],this.motionControl.getDestinationCoord()[1]);		
						this.visual.addDebugLog(DataChecker.moveInfo,this.motionControl.getDistance(),this.motionControl.getRotation(),this.motionControl.getTrigo());
						this.visual.addDebugLog(DataChecker.movePulses,distance,rotation,this.motionControl.getTrigo());
						/************************************/
						
						/*****Send move message to robot*****/
						if (this.COMMUNICATION){
							boolean ack = false;
							//Send move command...
							MotionCommand motionCmd = new MotionCommand(MotionCommand.DEFAULT_API_TYPE, (int)distance, (int)rotation, (int)trigo);
							while (!ack) {
								transmission.txMotionCom(motionCmd);
								visual.addMsgLog("[SEND]\t[CMD]\t" + motionCmd.toHexPrint() + "\n");
								
								//Receive ack...
								Thread.sleep(1000);
								MotionCommand motionAck = reception.getMotionAck();
								if (motionAck != null) {
									if ((motionAck.getDistancePulse() == distance) && (motionAck.getRotationPulse() == rotation)) {
										if (Base.DEBUG) System.out.println("(executeRoutine0) Received ACK");
										visual.addMsgLog("[RECEIVED]\t[ACK]\t" + motionAck.toHexPrint() + "\n");
										ack = true;
									} else {
										if (Base.DEBUG) System.out.println("(executeRoutine0) ACK received not expected, retransmitting...");
									}
								} else {
									if (Base.DEBUG) System.out.println("(executeRoutine0) ACK not received, retransmitting...");
								}
							}
						}
						/************************************/
						
						/***Update new robot's position***/
						if (Base.DEBUG) System.out.println("(executeRoutine1) distance = "+df.format(distance)+", rotation = "+df.format(rotation)+", trigo = "+motionControl.getTrigo());
						this.rover.setNewPosition(distance, rotation, (int) trigo);
						this.visual.addDebugLog(DataChecker.newPosition,this.rover.getX(),this.rover.getY(),this.rover.getr());
						this.rover.storeCorrectPosition(trajectory);
						
						this.visual.updateDrawing(this.rover.getPositions(), this.nodes.getLandmarks(), this.rover.getErrorPositions(), this.rover.getEstimatePositions(), this.rover.getAntislamPositions(), this.motionControl.getPolygons());
						/************************************/
						
						if(Base.DEBUG) {
							System.out.println();
							Thread.sleep(200);
						} else {
							if (this.numberOfSimulations == 1)	Thread.sleep(50);
						}
					} //End if
				}//End while
				
				/******************************************************/
				/************************STOP INFO*********************/
				/******************************************************/	
				this.visual.addStopInfo(true,false);
			}//End for
		} catch (Exception e) {
			this.visual.reInitChecker();
			e.printStackTrace();
		}
	}
	
	/**
	 * Executes the 4 stage comparative simulation.
	 * The simulation is run 4 times each time with different components activated:
	 * 		1- motion plan
	 * 		2- motion plan & error
	 * 		3- motion plan & error & ekf
	 * 		4- motion plan & anti-slam & error & ekf
	 */
	public void executeRoutine1(){	
		
		/***Simulation's variables declaration***/
		double [] tempReceive = null;
		double [] tempLandmark = null;
		double [] receiveCoord = new double[2];
		double [] antiSlamedCoord = new double[2];
		double [] errCoord, finalCoord;
		ListIterator<double[]> li;
		double distance, rotation, trigo;
		int coordIndex = 0, step = 0;
		DecimalFormat df = new DecimalFormat("#####.##");
		/****************************************/
		
		/***Init components***/
		initMain();
		initMotion();
		if (this.COMMUNICATION) initCommunication();
		/*********************/
		
		try {
			for (int trajectory=0;trajectory<4;trajectory++){
				
				/******************************************************/
				/********************INITIALISATION********************/
				/******************************************************/
				//Initialize robot
				if (this.scenario == 1) this.rover.setPosition(Base.ROBOTINITIALPOSITION_SC1[0], Base.ROBOTINITIALPOSITION_SC1[1], 0);
				else this.rover.setPosition(Base.ROBOTINITIALPOSITION[0], Base.ROBOTINITIALPOSITION[1], 0);
				this.rover.storeCorrectPosition(trajectory);
				initMotion();
				
				//reInitialize landmarks
				this.nodes.reInitializeLandmarks();
				
				//Initialize error (recording or pool)
				step = 0;
				this.isDetected = false;
				
				switch (trajectory){
					case Base.INITIALTRAJECTORY:
						if (Base.DEBUG) System.out.println("(executeRoutine1) Motion Plan");
						
						//Initialize custom parameters
						this.enableError = false;
						this.enableEkf = false;
						this.antiSlam = false;
						break;
					case Base.ERRORTRAJECTORY:
						if (Base.DEBUG) System.out.println("(executeRoutine1) Motion Plan - error");						
						//Initialize custom parameters
						this.enableError = true;
						this.enableEkf = false;
						this.antiSlam = false;
						
						//Initialize error pool	
						initError();	
						this.storeError.generateErrorPool();
						break;
					case Base.EKFTRAJECTORY:
						if (Base.DEBUG) System.out.println("(executeRoutine1) Motion Plan - error - ekf");
						initEkf();
						
						//Initialize custom parameters
						this.enableError = true;
						this.enableEkf = true;
						this.antiSlam = false;
						break;
					case Base.ANTISLAMTRAJECTORY:
						if (Base.DEBUG) System.out.println("(executeRoutine1) Motion Plan - antislam - error - ekf");
						initEkf();
						initAntiSlam(this.antiSlamType);
						
						//Initialize custom parameters
						this.enableError = true;
						this.enableEkf = true;
						this.antiSlam = true;
						break;
					default:
						break;
				}
				
				/******************************************************/
				/************************RUNNING***********************/
				/******************************************************/
				while ((Base.euclideanDist(this.rover.getX(),this.rover.getY(),Base.FINALDESTINATION[0],Base.FINALDESTINATION[1])>Base.FINALDISTANCE)&&(!this.isDetected)&&(step<1000)) { //end
					/*********************main loop********************/
					if (!this.simulation){
						Thread.sleep(1000);
					} else {
						this.visual.addDebugLog(DataChecker.initialPosition, this.rover.getX(),this.rover.getY(),this.rover.getr());
						
						/***Learn landmark if in communication Range***/
						Vector<double[]> receivedLandmarks = null;
						if (this.COMMUNICATION) {	//In this case, realLandmarks = received landmarks
							receivedLandmarks = this.reception.getReceivedLandmarks();					
							//Displaying received landmarks
							ListIterator<double[]> receivedLandmarkList = receivedLandmarks.listIterator();
							while (receivedLandmarkList.hasNext()) {
								double[] landmark = receivedLandmarkList.next();
								this.visual.addMsgLog("[RECEIVED]\t[DATA]\t"+landmark[0]+"\t"+landmark[1]+"\t"+landmark[2]+"\n");
							}
						} else receivedLandmarks = this.realLandmarks; //Faking receiving landmark
						
						//Faking communication ranges: store only close landmarks
						ListIterator<double[]> landmarkList = receivedLandmarks.listIterator();
						while (landmarkList.hasNext()) {
							double[] landmark = landmarkList.next();
							if ((Base.euclideanDist(this.rover.getX(),this.rover.getY(),landmark[1],landmark[2])<=DataChecker.communicationRange)&&(!this.nodes.knownLandmark((int)landmark[0], landmark[1], landmark[2]))) this.nodes.storeLandmark((int)landmark[0], landmark[1], landmark[2]);	
							
						}
						//Set Real Landmarks in case of communication
						if (this.COMMUNICATION) {
							this.realLandmarks = this.nodes.getRealLandmarks();
							this.visual.setRealLandmarks(this.realLandmarks);
							if (this.antiSlam) this.counterSlam.setRealLandmarks(this.realLandmarks);
						}
						/*********************************************/
						
						/*****Process data reception*****/
						for (int j=0;j<this.nodes.size();j++){
							tempReceive = this.nodes.getRealLandmarks().get(j);
							//Receiving nodes' coord
							coordIndex = (int)tempReceive[0];
							receiveCoord[0] = tempReceive[1];
							receiveCoord[1] = tempReceive[2];
						
							//Apply Anti-SLAM error
							if (this.antiSlam){ 
								antiSlamedCoord = this.counterSlam.applyAntiSlam(coordIndex,receiveCoord,this.rover.getX(), this.rover.getY());
								this.visual.addDebugLog(DataChecker.antiSlam, coordIndex,receiveCoord[0],receiveCoord[1],antiSlamedCoord[0],antiSlamedCoord[1],this.antiSlamType);					
							} else {
								antiSlamedCoord = receiveCoord;
							}
						
							//Apply error
							if (this.enableError){
								double err = (this.storeError.getErrorFromPool(step))[1];
								if (Base.DEBUG) System.out.println("(executeRoutine1) Used error "+err+ " during step "+step);
								errCoord = this.mistaker.applyErrorFromPool(err, this.rover.getX(), this.rover.getY(), antiSlamedCoord[0], antiSlamedCoord[1]);
								this.visual.addDebugLog(DataChecker.error, coordIndex,receiveCoord[0],receiveCoord[1],errCoord[0],errCoord[1]);
								step++;
							} else {
								errCoord = antiSlamedCoord;
							}
							
							//Store final Coord
							finalCoord = errCoord;
							this.nodes.replaceLandmark(coordIndex, finalCoord[0], finalCoord[1]);
							tempReceive = null;
						}
			        
						//Display all landmarks
						li = this.nodes.getLandmarks().listIterator();
						if (Base.DEBUG) System.out.println("(executeRoutine1) We have "+this.nodes.size()+" landmarks");
						while(li.hasNext()){
							tempLandmark = li.next();
							this.visual.addDebugLog(DataChecker.landmark,tempLandmark[0],this.nodes.size(),tempLandmark[1],tempLandmark[2]);
						}			        
						/********************************/
					
						/***************Apply EKF***********/
						if ((this.enableEkf) && (this.nodes.size()>1)){	
							this.filter.setMotion(this.rover.getX(), this.rover.getY(), 0, 0);
					        this.filter.initMeasurement(this.nodes.getLandmarks());
					        this.filter.calcul();
					        this.rover.setPosition(this.filter.getMEstimate()[0][0], this.filter.getMEstimate()[1][0], 0);
					        this.rover.storeCorrectPosition(trajectory);	
							
					        this.visual.addDebugLog(DataChecker.ekf,this.rover.getX(),this.rover.getY(),0, 0);
					        if (Base.DEBUG) System.out.println("(executeRoutine1) Exit of EKF x: "+df.format(this.rover.getX())+ " Y: "+df.format(this.rover.getY())); 
						}
						/************************************/
						
				        /*****Calculate next motion step*****/
						this.motionControl.getNextPulses(this.rover.getX(),this.rover.getY(),this.rover.getr(),this.nodes);
						distance = this.motionControl.getPulse_dist();
						rotation = this.motionControl.getPulse_rot();
						trigo = this.motionControl.getTrigo();
						/************************************/
					
						/****************Logs***************/
						this.visual.addDebugLog(DataChecker.destination,this.motionControl.getDestinationCoord()[0],this.motionControl.getDestinationCoord()[1]);		
						this.visual.addDebugLog(DataChecker.moveInfo,this.motionControl.getDistance(),this.motionControl.getRotation(),this.motionControl.getTrigo());
						this.visual.addDebugLog(DataChecker.movePulses,distance,rotation,this.motionControl.getTrigo());
						/************************************/
						
						/*****Send move message to robot*****/
						if (this.COMMUNICATION){
							boolean ack = false;
							//Send move command...
							MotionCommand motionCmd = new MotionCommand(MotionCommand.DEFAULT_API_TYPE, (int)distance, (int)rotation, (int)trigo);
							while (!ack) {
								transmission.txMotionCom(motionCmd);
								visual.addMsgLog("[SEND]\t[CMD]\t" + motionCmd.toHexPrint() + "\n");
								
								//Receive ack...
								Thread.sleep(1000);
								MotionCommand motionAck = reception.getMotionAck();
								if (motionAck != null) {
									if ((motionAck.getDistancePulse() == distance) && (motionAck.getRotationPulse() == rotation)) {
										if (Base.DEBUG) System.out.println("(executeRoutine1) Received ACK");
										visual.addMsgLog("[RECEIVED]\t[ACK]\t" + motionAck.toHexPrint() + "\n");
										ack = true;
									} else {
										if (Base.DEBUG) System.out.println("(executeRoutine1) ACK received not expected, retransmitting...");
									}
								} else {
									if (Base.DEBUG) System.out.println("(executeRoutine1) ACK not received, retransmitting...");
								}
							}	
						}
						/************************************/
						
						/***Update new robot's position***/
						if (Base.DEBUG) System.out.println("(executeRoutine1) distance = "+df.format(distance)+", rotation = "+df.format(rotation)+", trigo = "+motionControl.getTrigo());
						this.rover.setNewPosition(distance, rotation, (int) trigo);
						this.visual.addDebugLog(DataChecker.newPosition,this.rover.getX(),this.rover.getY(),this.rover.getr());
						this.rover.storeCorrectPosition(trajectory);
						
						this.visual.updateDrawing(this.rover.getPositions(), this.nodes.getLandmarks(), this.rover.getErrorPositions(), this.rover.getEstimatePositions(), this.rover.getAntislamPositions(), this.motionControl.getPolygons());
						/************************************/
						
						if(Base.DEBUG) {
							System.out.println();
							Thread.sleep(200);
						} else {
							if (this.numberOfSimulations == 1)	Thread.sleep(50);
						}
					} //End if
				}//End while
				
				/******************************************************/
				/************************STOP INFO*********************/
				/******************************************************/	
				this.visual.addStopInfo(true,false);
			}//End for
		} catch (Exception e) {
			e.printStackTrace();
		}
	}
	
	/**
	 * Simple simulation mode.
	 * The simulation is run once. The components to be activated are specified by the user in GUI.
	 */
	public void executeRoutine2(){
		
		/***initialize components***/
		initMain();
		initMotion();		
		initError();	
		if (this.COMMUNICATION) initCommunication();
		if (this.enableEkf) initEkf();
		initAntiSlam(this.antiSlamType);
		/***************************/
		
		/***Simulation's variables declaration***/
		double [] tempReceive = null;
		double [] tempLandmark = null;
		double [] receiveCoord = new double[2];
		double [] antiSlamedCoord = new double[2];
		double [] errCoord, finalCoord;
		ListIterator<double[]> li;
		double distance, rotation, trigo;
		int coordIndex = 0;
		DecimalFormat df = new DecimalFormat("#####.##");
		/****************************************/
		
		try {
			
			/*********************main loop********************/
			while ((Base.euclideanDist(this.rover.getX(),this.rover.getY(),Base.FINALDESTINATION[0],Base.FINALDESTINATION[1])>Base.FINALDISTANCE)&&(!this.isDetected)){
				if (!this.simulation){		//Do not simulate, sleep some time
					Thread.sleep(1000);
				} else {
					this.visual.addDebugLog(DataChecker.initialPosition, this.rover.getX(),this.rover.getY(),this.rover.getr());
					
					/***Learn landmark if in communication Range***/
					Vector<double[]> receivedLandmarks = null;
					if (this.COMMUNICATION) {	//In this case, realLandmarks = received landmarks
						Thread.sleep(300);
						receivedLandmarks = this.reception.getReceivedLandmarks();					
						//Displaying received landmarks
						ListIterator<double[]> receivedLandmarkList = receivedLandmarks.listIterator();
						while (receivedLandmarkList.hasNext()) {
							double[] landmark = receivedLandmarkList.next();
							this.visual.addMsgLog("[RECEIVED]\t[DATA]\t"+landmark[0]+"\t"+landmark[1]+"\t"+landmark[2]+"\n");
						}
					} else receivedLandmarks = this.realLandmarks; //Faking receiving landmark
					
					//Faking communication ranges: store close landmarks
					ListIterator<double[]> landmarkList = receivedLandmarks.listIterator();
					while (landmarkList.hasNext()) {
						double[] landmark = landmarkList.next();
						if ((Base.euclideanDist(this.rover.getX(),this.rover.getY(),landmark[1],landmark[2])<=DataChecker.communicationRange)&&(!this.nodes.knownLandmark((int)landmark[0], landmark[1], landmark[2]))) this.nodes.storeLandmark((int)landmark[0], landmark[1], landmark[2]);	
						
					}
					//Set new Real Landmarks in case of communication
					if (this.COMMUNICATION) {
						this.realLandmarks = this.nodes.getRealLandmarks();
						this.visual.setRealLandmarks(this.realLandmarks);
						if (this.antiSlam) this.counterSlam.setRealLandmarks(this.realLandmarks);
					}
					/*********************************************/
					
					/*****Process data reception*****/
			        for (int j=0;j<this.nodes.size();j++){
			        	tempReceive = this.nodes.getRealLandmarks().get(j);
						//Faking receiving nodes' coordinates
						coordIndex = (int)tempReceive[0];
						receiveCoord[0] = tempReceive[1];
						receiveCoord[1] = tempReceive[2];
						if (Base.DEBUG) System.out.println("(executeRoutine2) Node "+coordIndex+"("+receiveCoord[0]+","+receiveCoord[1]+")");
						//Apply Anti-SLAM error
						if (this.antiSlam){
							antiSlamedCoord = this.counterSlam.applyAntiSlam(coordIndex,receiveCoord,this.rover.getX(), this.rover.getY());
							this.visual.addDebugLog(DataChecker.antiSlam, coordIndex,receiveCoord[0],receiveCoord[1],antiSlamedCoord[0],antiSlamedCoord[1],this.antiSlamType);
						} else {
							antiSlamedCoord = receiveCoord;
						}
						
						//Apply error
						if (this.enableError){
							errCoord = this.mistaker.applyError(this.rover.getX(), this.rover.getY(), antiSlamedCoord[0], antiSlamedCoord[1]);
							this.visual.addDebugLog(DataChecker.error, coordIndex,receiveCoord[0],receiveCoord[1],errCoord[0],errCoord[1]);
						} else {
							errCoord = antiSlamedCoord;
						}
						
						//Store final Coord
						finalCoord = errCoord;
						this.nodes.replaceLandmark(coordIndex, finalCoord[0], finalCoord[1]);
						tempReceive = null;
					}
			        
					//Display all landmarks
					if (Base.DEBUG) System.out.println("(executeRoutine2) We have "+this.nodes.size()+" landmarks");
			        li = this.nodes.getLandmarks().listIterator();
			        while(li.hasNext()){
			        	tempLandmark = li.next();
			        	this.visual.addDebugLog(DataChecker.landmark,tempLandmark[0],this.nodes.size(),tempLandmark[1],tempLandmark[2]);
			        }
			        /********************************/
					
					/***************Apply EKF***********/
			        if ((this.enableEkf) && (this.nodes.size()>1)){
			        	this.filter.setMotion(this.rover.getX(), this.rover.getY(), 0, 0);
				        this.filter.initMeasurement(this.nodes.getLandmarks());
				        this.filter.calcul();
				        this.rover.setPosition(this.filter.getMEstimate()[0][0], this.filter.getMEstimate()[1][0], 0);
				        this.rover.storeCorrectPosition(0);
				        this.visual.addDebugLog(DataChecker.ekf,this.rover.getX(),this.rover.getY(),0, 0);
				        if (Base.DEBUG) System.out.println("(executeRoutine2) Exit of EKF x: "+df.format(this.rover.getX())+ " Y: "+df.format(this.rover.getY()));
			        }
			        /**********************************/

			        /*****Calculate next motion step*****/
					this.motionControl.getNextPulses(this.rover.getX(),this.rover.getY(),this.rover.getr(),this.nodes);
					distance = this.motionControl.getPulse_dist();
					rotation = this.motionControl.getPulse_rot();
					trigo = this.motionControl.getTrigo();
					/************************************/
					
					/*****Logs*****/
					this.visual.addDebugLog(DataChecker.destination,this.motionControl.getDestinationCoord()[0],this.motionControl.getDestinationCoord()[1]);
					this.visual.addDebugLog(DataChecker.moveInfo,this.motionControl.getDistance(),this.motionControl.getRotation(),this.motionControl.getTrigo());
					this.visual.addDebugLog(DataChecker.movePulses,distance,rotation,this.motionControl.getTrigo());
					/**************/
					
					/******Send motion command to robot*****/
					if (this.COMMUNICATION){
						boolean ack = false;
						//Send move command...
						MotionCommand motionCmd = new MotionCommand(MotionCommand.DEFAULT_API_TYPE, (int)distance, (int)rotation, (int)trigo);
						while (!ack) {
							transmission.txMotionCom(motionCmd);
							visual.addMsgLog("[SEND]\t[CMD]\t" + motionCmd.toHexPrint() + "\n");
							
							//Receive ack...
							Thread.sleep(1000);
							MotionCommand motionAck = reception.getMotionAck();
							if (motionAck != null) {
								if ((motionAck.getDistancePulse() == distance) && (motionAck.getRotationPulse() == rotation)) {
									if (Base.DEBUG) System.out.println("(executeRoutine2) Received ACK");
									visual.addMsgLog("[RECEIVED]\t[ACK]\t" + motionAck.toHexPrint() + "\n");
									ack = true;
								} else {
									if (Base.DEBUG) System.out.println("(executeRoutine2) ACK received not expected, retransmitting...");
								}
							} else {
								if (Base.DEBUG) System.out.println("(executeRoutine2) ACK not received, retransmitting...");
							}
						}	
					}
					/***************************************/
					
					/****Update new robot's position****/
					if (Base.DEBUG) System.out.println("(executeRoutine2) distance = "+df.format(distance)+", rotation = "+df.format(rotation)+", trigo = "+trigo);
					this.rover.setNewPosition(distance, rotation, (int) trigo);
					this.visual.addDebugLog(DataChecker.newPosition,this.rover.getX(),this.rover.getY(),this.rover.getr());
					this.rover.storeCorrectPosition(0);
					if (Base.DEBUG) System.out.println("(executeRoutine2) After motion x: "+df.format(this.rover.getX())+ " y: "+df.format(this.rover.getY()));
					
					this.visual.updateDrawing(this.rover.getPositions(), this.nodes.getLandmarks(), null, this.rover.getEstimatePositions(), null, this.motionControl.getPolygons());
					/************************************/
					
					if(Base.DEBUG) {
						System.out.println();
						Thread.sleep(200);
					} else {
						if (this.numberOfSimulations == 1)	Thread.sleep(50);
						else Thread.sleep(10);
					}
				}//end if
			}//end while
			/*****Add stop informations (metrics) & save them*****/
			this.visual.addStopInfo(true,false);
			/*****************************************************/
		} catch (Exception e) {
			e.printStackTrace();
		}
	}
	
	/**
	 * Calculate the euclidean distance between points (x1, y1) and (x2, y2)
	 * @param x1 the X coordinate of first point
	 * @param y1 the Y coordinate of first point
	 * @param x2 the X coordinate of second point
	 * @param y2 the Y coordinate of second point
	 * @return the euclidean distance between (x1, y1) & (x2, y2)
	 */	
	public static final double euclideanDist(double x1, double y1, double x2, double y2){
		return(Math.sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1)));
	}

}
